#include "Box.h"
#include "Spring.h"

#include <cstdio>

Box::Box(const AABB& aabb, double invMass, double stiffness)
{
	const Vector3& c1 = aabb.corner1;
	const Vector3& c2 = aabb.corner2;
	//fprintf(stderr, "aabb %.3lf %.3lf %.3lf %.3lf %.3lf %.3lf\n", c1.x, c1.y, c1.z, c2.x, c2.y, c2.z);
	/*
	 * Define the particles.
	 * They are indexed as follows:
	 *
	 * 2--------3
	 * |\       |\
	 * | 6--------7
	 * | |      | |
	 * 0-|------1 |
	 *  \|       \|
	 *   4--------5
	 *
	 * where 0 is c1 and 7 is c2.
	 */
	Vector3 pos;
	for(int i = 0; i < 8; ++i) {
		pos.x = ((i & 1) == 0)? c1.x : c2.x;
		pos.y = ((i & 2) == 0)? c1.y : c2.y;
		pos.z = ((i & 4) == 0)? c1.z : c2.z;
		//fprintf(stderr, "p[%d] %.3lf %.3lf %.3lf\n", i, pos.x, pos.y, pos.z);
		//fprintf(stderr, "%d %d %d\n", i&1, i&2, i&4);
		p[i] = new Particle(pos, invMass);
		particle.insert(p[i]);
	}
	//p[0]->invMass = 0;
	/*
	 * Define the constraints.
	 * It would have been cleaner to create a volume preservation constraint
	 * but it's easier this way.
	 */
	for(int i = 0; i < 4; ++i) {
		// all constraints along (Ox)
		constraint.insert(new Spring(p[2*i], p[2*i+1], stiffness));
		// all constraints along (Oy)
		int j = i < 2? i : i+2;
		constraint.insert(new Spring(p[j], p[j+2], stiffness));
		// all constraints along (Oz)
		constraint.insert(new Spring(p[i], p[i+4], stiffness));
		// diagonals
		constraint.insert(new Spring(p[i], p[7-i], stiffness));
	}

	/*
	 * Define the collision surface.
	 * 12 Triangles for the 6 sides.
	 */
	triangle.insert(new Triangle(p[0], p[1], p[2]));
	triangle.insert(new Triangle(p[1], p[2], p[3]));
	triangle.insert(new Triangle(p[4], p[5], p[6]));
	triangle.insert(new Triangle(p[5], p[6], p[7]));
	triangle.insert(new Triangle(p[0], p[2], p[4]));
	triangle.insert(new Triangle(p[2], p[4], p[6]));
	triangle.insert(new Triangle(p[1], p[3], p[5]));
	triangle.insert(new Triangle(p[3], p[5], p[7]));
	triangle.insert(new Triangle(p[0], p[1], p[4]));
	triangle.insert(new Triangle(p[1], p[4], p[5]));
	triangle.insert(new Triangle(p[2], p[3], p[6]));
	triangle.insert(new Triangle(p[3], p[6], p[7]));
}

//------------------------------------------------------------------------------

void Box::accumulateForces()
{
	//p[0]->acceleration = Vector3();
}

//------------------------------------------------------------------------------

